#ifndef __M3N_RANDOM_FIELD_H__
#define __M3N_RANDOM_FIELD_H__
/*********************************************************************
 * Software License Agreement (Modified BSD License)
 *
 * Copyright (c) 2009-2010, Willow Garage, Daniel Munoz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the copyright holders' organizations nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include <cstdlib>

#include <list>
#include <vector>
#include <map>
#include <set>
#include <fstream>
#include <sstream>
#include <string>
#include <limits>
#include <iostream>

// --------------------------------------------------------------
/*!
 * \file random_field.h
 *
 * \brief A class representing primitives and their features to
 *        contextually classify.
 */
// --------------------------------------------------------------

// --------------------------------------------------------------
//* RandomField
/*!
 * \brief A class that represents a random field of nodes and cliques
 *
 * Conceptually, nodes are the primitives things you wish to classify and
 * cliques describe regions/groups of interacting primitives.  Example:
 * for image segmentation, pixels are the nodes, cliques of size 2 (edges)
 * are usually created among neighboring pixels, and high-order cliques over
 * segments of pixels.
 *
 * \warning Should always be constructed with createNode and createClique methods
 */
// --------------------------------------------------------------
class RandomField
{

  public:
    /*! \brief A constant value used when a Node has an unknown label */
    const static unsigned int UNKNOWN_LABEL;

    // --------------------------------------------------------------
    /*! \see Node definition below */
    // --------------------------------------------------------------
    class Node;

    // --------------------------------------------------------------
    /*! \see Clique definition below */
    // --------------------------------------------------------------
    class Clique;

    // --------------------------------------------------------------
    /*!
     * \brief Instantiates an empty RandomField with 0 clique-sets
     *
     * This constructor should be used when loading a RandomField from file.
     * (All clique-set information will be retrieve from the file)
     */
    // --------------------------------------------------------------
    RandomField();

    // --------------------------------------------------------------
    /*!
     * \brief Instantiates an empty RandomField
     *
     * \param nbr_clique_sets The number of clique sets this RandomField will contain
     *                        (the set of nodes is EXCLUDED)
     */
    // --------------------------------------------------------------
    RandomField(unsigned int nbr_clique_sets);

    // --------------------------------------------------------------
    /*!
     * \brief Destroys this RandomField
     */
    // --------------------------------------------------------------
    ~RandomField();

    // ===================================================================
    /*! \name Mutators  */
    // ===================================================================
    //@{
    // --------------------------------------------------------------
    /*!
     * \brief Frees all nodes and cliques contained in this random field,
     *        but does not change number of clique sets
     */
    // --------------------------------------------------------------
    void clear();

    // --------------------------------------------------------------
    /*!
     * \brief Creates a new Node with the given features and adds it to this RandomField
     *
     * This RandomField will generate a unique id for the Node.
     * The coordinates of the Node are not used in any way by this library.
     *
     * \param feature_vals An array of feature values that describe the node
     * \param label (Optional) The label of the Node
     * \param x (Optional) The x-coordinate the Node
     * \param y (Optional) The y-coordinate the Node
     * \param z (Optional) The z-coordinate the Node
     *
     * \return a pointer to the newly created Node on success, otherwise NULL
     *         on failure
     */
    // --------------------------------------------------------------
    const Node* createNode(const std::vector<double>& feature_vals,
                           unsigned int label = UNKNOWN_LABEL,
                           double x = 0.0,
                           double y = 0.0,
                           double z = 0.0);

    // --------------------------------------------------------------
    /*!
     * \brief Creates a new Node with the given features and id and adds it to this RandomField
     *
     * This call will fail if there exists another Node in this RandomField with the requested id.
     * The coordinates of the Node are not used in any way by this library.
     *
     * \param node_id The desired id for the Node
     * \param feature_vals An array of feature values that describe the node
     * \param label (Optional) The label of the Node
     * \param x (Optional) The x-coordinate the Node
     * \param y (Optional) The y-coordinate the Node
     * \param z (Optional) The z-coordinate the Node
     *
     * \return a pointer to the newly created Node on success, otherwise NULL
     *         on failure
     */
    // --------------------------------------------------------------
    const Node* createNode(const unsigned int node_id,
                           const std::vector<double>& feature_vals,
                           unsigned int label = UNKNOWN_LABEL,
                           double x = 0.0,
                           double y = 0.0,
                           double z = 0.0);

    // --------------------------------------------------------------
    /*!
     * \brief Creates a new Clique with the given features and adds it
     *        to the specified clique-set in this RandomField
     *
     * This RandomField will generate a unique id for the Clique in its
     * specified clique-set.
     * The coordinates of the Clique are not used in any way by this library.
     *
     * \param clique_set_idx The index of the clique-set the Clique should belong
     *                       to (starting from 0)
     * \param nodes The list of Nodes contained in the Clique
     * \param feature_vals An array of feature values that describe the clique
     * \param x (Optional) The x-coordinate the Clique
     * \param y (Optional) The y-coordinate the Clique
     * \param z (Optional) The z-coordinate the Clique
     *
     * \return a pointer to the newly created Clique on success, otherwise NULL
     *         on failure
     */
    // --------------------------------------------------------------
    const Clique* createClique(const unsigned int clique_set_idx,
                               const std::list<const Node*>& nodes,
                               const std::vector<double>& feature_vals,
                               double x = 0.0,
                               double y = 0.0,
                               double z = 0.0);

    // --------------------------------------------------------------
    /*!
     * \brief Creates a new Clique with the given features and id, and
     *        then adds it to the specified clique-set in this RandomField
     *
     * This call will fail if there exists another Clique in the specified
     * clique-set with the requested id.
     * The coordinates of the Clique are not used in any way by this library.
     *
     * \param clique_id The desired id of the Clique in the clique-set
     * \param clique_set_idx The index of the clique-set the Clique should belong
     *                       to (starting from 0)
     * \param nodes The list of Nodes contained in the Clique
     * \param feature_vals An array of feature values that describe the clique
     * \param x (Optional) The x-coordinate the Clique
     * \param y (Optional) The y-coordinate the Clique
     * \param z (Optional) The z-coordinate the Clique
     *
     * \return a pointer to the newly created Clique on success, otherwise NULL
     *         on failure
     */
    // --------------------------------------------------------------
    const Clique* createClique(const unsigned int clique_id,
                               const unsigned int clique_set_idx,
                               const std::list<const Node*>& nodes,
                               const std::vector<double>& feature_vals,
                               double x = 0.0,
                               double y = 0.0,
                               double z = 0.0);

    // --------------------------------------------------------------
    /*!
     * \brief Updates the labels for each Node contained in this RandomField.
     *
     * This method would be called after performing inference.
     *
     * \warning This is a somewhat slow call as all label information contained
     * in the cliques also need to be updated
     *
     * \param new_labeling Mapping of node ids to their new labels
     *
     * \return 0 on success, otherwise negative value on error
     */
    // --------------------------------------------------------------
    int updateLabelings(const std::map<unsigned int, unsigned int>& new_labeling);
    //@}

    // ===================================================================
    /*! \name Accessors  */
    // ===================================================================
    //@{
    // --------------------------------------------------------------
    /*!
     * \brief Returns mapping of ids assigned from this RandomField to associated primitives
     */
    // --------------------------------------------------------------
    inline const std::map<unsigned int, Node*>& getNodes() const
    {
      return m_nodes;
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns vector of clique-sets, which are represented by a mapping from clique ids
     * assigned from this RandomField to associated cliques
     */
    // --------------------------------------------------------------
    inline const std::vector<std::map<unsigned int, Clique*> >& getCliqueSets() const
    {
      return m_clique_sets;
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the number of nodes in this RandomField
     */
    // --------------------------------------------------------------
    inline unsigned int getNumberOfNodes() const
    {
      return m_nodes.size();
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the number of clique-sets in this RandomField
     */
    // --------------------------------------------------------------
    inline unsigned int getNumberOfCliqueSets() const
    {
      return m_clique_sets.size();
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the dimension for the node features
     */
    // --------------------------------------------------------------
    inline size_t getNodeDim() const
    {
      return m_node_dim;
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the list of feature dimensions for each clique set
     */
    // --------------------------------------------------------------
    inline const std::vector<size_t>& getCliqueSetDims() const
    {
      return m_clique_set_dims;
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the list of labels present in this RandomField
     */
    // --------------------------------------------------------------
    inline const std::set<unsigned int>& getLabels() const
    {
      return m_labels;
    }

    // --------------------------------------------------------------
    /*!
     * \brief Returns the flag if RandomField is fully labeled
     */
    // --------------------------------------------------------------
    inline bool isFullyLabeled() const
    {
      return m_fully_labeled;
    }
    //@}

    // ===================================================================
    /*! \name File I/O  */
    // ===================================================================
    //@{
    // --------------------------------------------------------------
    /*!
     * \brief Saves the features of all Nodes in this RandomField to file
     *
     * File format: x y z node_id label nbr_features [features]
     *
     * \param filename The filename to save to
     *
     * \return 0 on success, otherwise negative value on error
     */
    // --------------------------------------------------------------
    int saveNodeFeatures(std::string filename) const;

    // --------------------------------------------------------------
    /*!
     * \brief Saves the features of all Cliques in this RandomField to file
     *
     * The Cliques from clique-set i written in separate files with filename
     * suffix <basename>_cs_i.features
     *
     * File format: x y z clique_set_idx clique_id nbr_features [features]
     *
     * \param basename The basename of the output files
     *
     * \return 0 on success, otherwise negative value on error
     */
    // --------------------------------------------------------------
    int saveCliqueFeatures(std::string basename) const;

    // --------------------------------------------------------------
    /*!
     * \brief Saves the contents of this RandomField to file
     *
     * This is the random field file format used in the ROS 
     * functional_m3n package as of Sept 20, 2009
     *
     * This call will produce a file named <basename>.random_field and
     * multiple associative files for the nodes (.node_features) and
     * clique sets (._cs_XXX_features).
     * See the header of the files for the file format.
     *
     * To avoid loading/saving issues, the basename should contain the
     * full path of the file.
     *
     * \param basename The basename of the output files
     *
     * \return 0 on success, otherwise negative value on error
     */
    // --------------------------------------------------------------
    int saveRandomFieldWGASCII(std::string basename) const;

    // --------------------------------------------------------------
    /*!
     * \brief Clears this RandomField and then loads the contents of
     *        the previously saved RandomField into this RandomField
     *
     * This method uses the random field file format used in the ROS
     * functional_m3n package as of Sept 20, 2009
     *
     * \param basename The basename used during saveRandomField()
     *
     * \return 0 on success, otherwise negative value on error
     */
    // --------------------------------------------------------------
    int loadRandomFieldWGASCII(std::string basename);
    //@}

  private:
    /*! \brief Mapping from a Node's id in this RandomField to its instance */
    std::map<unsigned int, Node*> m_nodes;

    /*! \brief Vector of containers for each clique set.  Each container is
     *         a mapping from a Clique's id wrt the clique set to its instance */
    std::vector<std::map<unsigned int, Clique*> > m_clique_sets;

    /*! \brief Node feature dimension */
    size_t m_node_dim;

    /*! \brief Feature dimension per clique set*/
    std::vector<size_t> m_clique_set_dims;

    /*! \brief Labels contained in this RandomField */
    std::set<unsigned int> m_labels;

    /*! \brief A flag if all Nodes in the RandomField are labeled */
    bool m_fully_labeled;

  public:
    // --------------------------------------------------------------
    //* GenericClique
    /**
     * \brief Generic properties of any Node or Clique in the RandomField
     */
    // --------------------------------------------------------------
    class GenericClique
    {
        // Allow RandomField to access the protected functions
        friend class RandomField;

      public:
        // --------------------------------------------------------------
        /*!
         * \brief Instantiates a GenericClique with id 0 at location (0,0,0)
         */
        // --------------------------------------------------------------

        GenericClique() :
          m_id(0), m_x(0), m_y(0), m_z(0)
        {
        }

        // --------------------------------------------------------------
        /*!
         * \brief Deallocates feature memory assigned to this GenericClique
         */
        // --------------------------------------------------------------
        virtual ~GenericClique() = 0;

        // --------------------------------------------------------------
        /*!
         * \brief Returns the unique RandomField id of the node
         */
        // --------------------------------------------------------------
        inline unsigned int getID() const
        {
          return m_id;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the values of the features contained in the descriptors in vector format
         */
        // --------------------------------------------------------------
        inline const std::vector<double>& getFeatureVals() const
        {
          return m_feature_vals;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the number of feature values in vector format
         */
        // --------------------------------------------------------------
        inline unsigned int getNumberFeatureVals() const
        {
          return m_feature_vals.size();
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the x coordinate of this Node or Clique
         */
        // --------------------------------------------------------------
        inline double getX() const
        {
          return m_x;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the y coordinate of this Node or Clique
         */
        // --------------------------------------------------------------
        inline double getY() const
        {
          return m_y;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the z coordinate of this Node or Clique
         */
        // --------------------------------------------------------------
        inline double getZ() const
        {
          return m_z;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Defines the x y z coordinates for the GenericClique.
         *
         * These coordinates are NOT used in the learning and inference
         * procedures and are only here for convenience.
         *
         * \param x The x coordinate
         * \param y The y coordinate
         * \param z The z coordinate
         */
        // --------------------------------------------------------------
        inline void setXYZ(double x,
                           double y,
                           double z)
        {
          m_x = x;
          m_y = y;
          m_z = z;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Defines the features for this GenericClique
         *
         * \warning Make sure not to change the dimension of the features.
         *          There is no need to use this method except in RFUtil
         *
         * \param feature_vals Pointer to vector of feature values
         */
        // --------------------------------------------------------------
        inline void setFeatures(const std::vector<double>& feature_vals)
        {
          m_feature_vals = feature_vals;
        }

      protected:
        unsigned int m_id;
        std::vector<double> m_feature_vals;
        double m_x;
        double m_y;
        double m_z;
    };

    // --------------------------------------------------------------
    //* Node
    /**
     * \brief Represents the primitive to classify
     */
    // --------------------------------------------------------------
    class Node: public GenericClique
    {
        // Allow RandomField to access the protected functions
        friend class RandomField;

      public:
        // --------------------------------------------------------------
        /*!
         * \brief Instantiates a Node with a specified id.
         *
         * \warning The Node cannot be added to a RandomField that contains another node
         *          with same id.
         *
         * \param id The id of the node
         * \param label (Optional) The label of the Node
         */
        // --------------------------------------------------------------
        Node(unsigned int id,
             unsigned int label = UNKNOWN_LABEL) :
          m_label(label)
        {
          m_id = id;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the label of the node
         */
        // --------------------------------------------------------------
        inline unsigned int getLabel() const
        {
          return m_label;
        }

      protected:
        // --------------------------------------------------------------
        /*!
         * \brief Sets the label of this Node
         *
         * \param new_label The new label
         *
         * \warning Use with caution. Should afterwards call RandomField::Clique::updateLabels
         */
        // --------------------------------------------------------------
        inline void setLabel(const unsigned int new_label)
        {
          m_label = new_label;
        }

      private:
        unsigned int m_label;
    };

    // --------------------------------------------------------------
    //* Node
    /**
     * \brief Represents a group of primitives
     */
    // --------------------------------------------------------------
    class Clique: public GenericClique
    {
        // Allow RandomField to access the protected functions
        friend class RandomField;

      public:
        // --------------------------------------------------------------
        /*!
         * \brief \see GenericClique
         */
        // --------------------------------------------------------------
        Clique()
        {
        }

        // --------------------------------------------------------------
        /*!
         * \brief Instantiates a Clique with a specified id.
         *
         * \warning The Clique cannot be added to a RandomField that contains another clique
         *          with same id (within the same clique set).
         */
        // --------------------------------------------------------------
        Clique(const unsigned int id)
        {
          m_id = id;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the number of nodes contained in this Clique
         */
        // --------------------------------------------------------------
        inline unsigned int getOrder() const
        {
          return m_node_ids.size();
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the node ids contained in this Clique
         */
        // --------------------------------------------------------------
        inline const std::list<unsigned int>& getNodeIDs() const
        {
          return m_node_ids;
        }

        // --------------------------------------------------------------
        /*!
         * \brief Returns the mode and second mode labels contained in this Clique
         *
         * mode2_label may equal RandomField::UNKNOWN_LABEL if all nodes in the clique are
         * labeled the same (and mode2_count will be 0)
         *
         * \param mode1_label Reference to store the mode label
         * \param mode1_count Reference to store the number of nodes with mode1_label
         * \param mode2_label Reference to store the 2nd mode label
         * \param mode2_count Reference to store the number of nodes with mode2_label
         * \param mode1_node_ids (Optional) Pointer to store list of node ids that are labeled mode1_label
         * \param tempo_labeling (Optional) Instead of using internal label information to compute
         *                       modes, will act as if each contained node is labeled using this
         *                       map of node_id -> label.  Use NULL to not use.
         *
         * \return 0 on success, otherwise negative value
         */
        // --------------------------------------------------------------
        int getModeLabels(unsigned int& mode1_label,
                          unsigned int& mode1_count,
                          unsigned int& mode2_label,
                          unsigned int& mode2_count,
                          std::list<unsigned int>* mode1_node_ids = NULL,
                          const std::map<unsigned int, unsigned int>* tempo_labeling = NULL) const;

      protected:
        // --------------------------------------------------------------
        /*!
         * \brief Adds the given Node to this Clique
         *
         * \param new_node The new node to add to this Clique
         */
        // --------------------------------------------------------------
        void addNode(const Node& new_node);

        // --------------------------------------------------------------
        /*!
         * \brief Updates the information regarding what labels are contained in this Clique
         *
         * \param node_labels Mapping from node id to label. Map size can be bigger than clique order.
         *
         * \warning Use with caution.  Should be called after RandomField::Node::setLabel
         *
         * \return 0 on success, otherwise negative value on error.
         */
        // --------------------------------------------------------------
        int updateLabels(const std::map<unsigned int, unsigned int>& node_labels);

      private:
        std::list<unsigned int> m_node_ids;
        std::map<unsigned int, std::list<unsigned int> > m_labels_to_node_ids;
    };

};
#endif
